package Roboter;

import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;

public class Rotationsroboter {
	private Linearachse linearachse;
	private Rotationsachse rotationsachse;
	private Hubservo hubservo;
	private LinearPoti linearPoti;
	private RotationsPoti rotationsPoti;
	private HubPoti hubPoti;
	private Elektromagnet elektromagnet;
	private Bewegung bewegung;
	private boolean wiederholen;
	
	public Rotationsroboter() {
		linearachse = new Linearachse();
		rotationsachse = new Rotationsachse();
		hubservo = new Hubservo();
		linearPoti = new LinearPoti();
		rotationsPoti = new RotationsPoti();
		hubPoti = new HubPoti();
		elektromagnet = new Elektromagnet();
		bewegung = new Bewegung(linearachse, linearPoti, rotationsachse, rotationsPoti, hubservo, hubPoti, elektromagnet);
		
		//Roboter auf Startposition ausrichten
		bewegung.hubAuf();
		bewegung.startPosition();
		
		//Z-Achse senken, Positionen einlesen
		bewegung.hubAb();
		try {
			Thread.sleep(150);
		} catch (InterruptedException e) {
			
		}
		rotationsachse.spannungsfrei();
		hubservo.spannungsfrei();
		Positionen positionen = new Positionen(linearPoti, rotationsPoti, linearachse);
		
		//Z-Achse heben, Bewegungsablaufstarten
		bewegung.hubAuf();
		System.out.println("Bewegungsablauf starten:");
		System.out.println("[Enter]");
		Button.ENTER.waitForPress();
		LCD.clear();
		this.wiederholen = true;
		while(wiederholen) {
			for(int i = 0; i < 3; i++) {
				bewegung.wuerfelVerschieben(positionen.gibPosition2(), positionen.gibPosition3());
				bewegung.wuerfelVerschieben(positionen.gibPosition1(), positionen.gibPosition2());
				bewegung.wuerfelVerschieben(positionen.gibPosition3(), positionen.gibPosition1());
			}
			
			System.out.println("Wiederholen?");
			System.out.println("[Enter]");
			Button.ESCAPE.addButtonListener(new ButtonListener() {
				
				public void buttonReleased(Button arg0) {
					
				}
				
				public void buttonPressed(Button arg0) {
					wiederholen = false;
				}
			});
			Button.ENTER.waitForPress();
			LCD.clear();
		}
		
	}
	
	public static void main(String[] args) {
		new Rotationsroboter();
	}
}
